/*
 * Gluon.cpp
 *
 *  Created on: 11.03.2012
 *      Author: ignacio
 */

#include "Gluon.h"

#include "timing.h"
#include <sstream>
using std::stringstream;

namespace neurignacio{
	void Gluon::addMovementBehaviour(MovementBehaviour* behaviour)
	// Replaces any Behaviour Set before
	{
		if (behaviour)
		{
//			cout << "Adding: " << behaviour->MovementBehaviour_ID_Description() << endl;
			movementBehaviour.push_back(behaviour);
			//			movementBehaviour.push_back(behaviour);
//			movementBehaviour.back()->character=&(*this); // I set the character in getSteering()
		}
	}

	void Gluon::addMovementBehaviour(MovementBehaviourPointerContainer& behaviourArray)
	{
		MovementBehaviourPointerContainer::iterator behaviour;
		for (behaviour=behaviourArray.begin();behaviour!=behaviourArray.end(); ++behaviour)
			addMovementBehaviour(*behaviour);
	}

	void Gluon::sortMovementBehaviours(void)
	{
		sort(movementBehaviour.rbegin(), movementBehaviour.rend()); // Reverse sorting of Priorities
	}

	void Gluon::getSteering(const SteeringOutput& externalSteering)
	// Includes Weighted Blending and Priorities
	{
		stringstream out;
		sortMovementBehaviours();
		// Steering is initialized
		steering.clear();
		MovementBehaviourPointerContainer::iterator behaviour=movementBehaviour.begin();
		real priority = (*behaviour)->priority;
		out << "\t!!!!! GLUON::GETSTEERING - Line " << __LINE__ << endl;
		out << "\tnumber of behaviours: "<< movementBehaviour.size() << " priority: " << priority << endl;
		out << "\tlinear Epsilon: " << linearEpsilon << " angular Epsilon: " << angularEpsilon << endl;
		SteeringOutput steerPriorityGroup; // Holds the blended steering of a same priority group (i.e. a group with identical priority)
		while (behaviour!=movementBehaviour.end())
		{
			if ((*behaviour)->priority == priority)
			{
//				out << "Checking Behaviour! Line "<< __LINE__ << endl;
				(*behaviour)->character=&(*this);	// I'm always the character of my behaviour!!!!
				SteeringOutput steer; // Hold the steering of single movement behaviour
				(*behaviour)->getSteering(steer);
				out << "\t" << (*behaviour)->MovementBehaviour_ID_Description() << " steer linear: " << steer.linear << " angular: " << steer.angular << endl;
				steerPriorityGroup.linear += steer.linear * ((*behaviour)->weight);
				steerPriorityGroup.angular += steer.angular * ((*behaviour)->weight);
				out << "\tsteerPriorityGroup linear: " << steerPriorityGroup.linear << " angular: " << steerPriorityGroup.angular << endl;
				++behaviour;
			}
			else // we change the priority group
			{
				// If the steering of the priority group that we are leaving now is bigger than the epsilon paramenters,
				// we skip the lower priority groups
				out << "\tSteering(PriorityGroup) linear: " << steerPriorityGroup.linear.squareMagnitude() << " angular: " << neurignacio::abs(steering.angular) << endl;
				if (steerPriorityGroup.linear.squareMagnitude() > (linearEpsilon*linearEpsilon) || (neurignacio::abs(steerPriorityGroup.angular)>angularEpsilon))
				{
					break; // No further calculations are needed
				}
				else // we move to the next priority group
				{
					steerPriorityGroup.clear();
					priority = (*behaviour)->priority;
				}
			}
		}
		steering.linear = steerPriorityGroup.linear + initialSteering.linear + externalSteering.linear;
		steering.angular = steerPriorityGroup.angular + initialSteering.angular + externalSteering.angular;
		out << "\tGLUON::steering linear: " << steering.linear << " angular: " << steering.angular << endl;
//		cout << out;
	}

	int Gluon::whichMovementBehaviour(const unsigned int& index) const
	{
//		cout << "Which Movement Behaviour? index="<< index << " size="<< movementBehaviour.size() << endl;
		if (index>0 && index<movementBehaviour.size())
		{
			return movementBehaviour[index]->getID();
		}
		else
			return -1;
	}

	void Gluon::trim()
	{
		trimMaxSpeed(maxSpeed);
		trimMaxRotation(maxRotation);
		if (velocity.squareMagnitude()<minVelocity*minVelocity) // If velocity is too low, consider zero
			velocity.clear();
	}

	void Gluon::refresh(void)
	{
		stringstream out(stringstream::in | stringstream::out);
		float duration=neurignacio::TimingData::get().lastFrameDuration*0.001f;

		// Update the steering if we have a movement algorithm
//	    getSteering();
//		cout << steer << endl;
		integrate(steering, duration);
//		setOrientationFromVelocity(); // Orientation will be calculated from Steering Behaviour when correspond!
		trim();

		out << "\t---------- Refresh Line: " << __LINE__ << endl;
		out << "\tVelocity: " << velocity.magnitude() << " Orientation: " << orientation << endl;
		// Normalise the orientation, unless we're wandering (in which
		// case this will destroy the wander behaviour).
/*		if (movementBehaviour->getID()==neurignacio::KINEMATIC_WANDER)
			setOrientationFromVelocity(steering.linear);
*/
//		cout << out.str();
	}

ostream& operator<<(ostream& out, const Gluon& gluon)
{
	// const_iterator because gluon is const
	for (MovementBehaviourPointerContainer::const_iterator behaviour=gluon.movementBehaviour.begin();behaviour!=gluon.movementBehaviour.end(); ++behaviour)
	{
		out << (*behaviour)->MovementBehaviour_ID_Description() << ".";
	}
	out << "{" << static_cast<Muon>(gluon) <<"}";
	out << "->{" << gluon.steering << "}";
	return out;
}

}; // end of namespace neurignacio
